<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.12"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>Os1: /home/alejandro/Desarrollo eclipse/os1/Thirdparty/g2o/g2o/core/block_solver.hpp Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
</script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Os1
   &#160;<span id="projectnumber">1</span>
   </div>
   <div id="projectbrief">OrbSlam2 documentado y modificado</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.12 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('',true,'Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('block__solver_8hpp_source.html','');});
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">block_solver.hpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="block__solver_8hpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="comment">// g2o - General Graph Optimization</span></div><div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;<span class="comment">// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard</span></div><div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment">// All rights reserved.</span></div><div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;<span class="comment">//</span></div><div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment">// Redistribution and use in source and binary forms, with or without</span></div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="comment">// modification, are permitted provided that the following conditions are</span></div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment">// met:</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="comment">//</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="comment">// * Redistributions of source code must retain the above copyright notice,</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="comment">//   this list of conditions and the following disclaimer.</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="comment">// * Redistributions in binary form must reproduce the above copyright</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="comment">//   notice, this list of conditions and the following disclaimer in the</span></div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="comment">//   documentation and/or other materials provided with the distribution.</span></div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="comment">//</span></div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="comment">// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS &quot;AS</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="comment">// IS&quot; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED</span></div><div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;<span class="comment">// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A</span></div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="comment">// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT</span></div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;<span class="comment">// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,</span></div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;<span class="comment">// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED</span></div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;<span class="comment">// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR</span></div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;<span class="comment">// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF</span></div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;<span class="comment">// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING</span></div><div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;<span class="comment">// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS</span></div><div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;<span class="comment">// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</span></div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="sparse__optimizer_8h.html">sparse_optimizer.h</a>&quot;</span></div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;<span class="preprocessor">#include &lt;eigen3/Eigen/LU&gt;</span></div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;<span class="preprocessor">#include &lt;fstream&gt;</span></div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;<span class="preprocessor">#include &lt;iomanip&gt;</span></div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;<span class="preprocessor">#include &quot;../stuff/timeutil.h&quot;</span></div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;<span class="preprocessor">#include &quot;../stuff/macros.h&quot;</span></div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;<span class="preprocessor">#include &quot;../stuff/misc.h&quot;</span></div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;</div><div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;<span class="keyword">namespace </span><a class="code" href="namespaceg2o.html">g2o</a> {</div><div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;</div><div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;<span class="keyword">using namespace </span><a class="code" href="namespacestd.html">std</a>;</div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;<span class="keyword">using namespace </span><a class="code" href="namespace_eigen.html">Eigen</a>;</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;</div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00042"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a04701a223a14708c9c84ce4d7e7af3f6">   42</a></span>&#160;<a class="code" href="classg2o_1_1_block_solver.html#a04701a223a14708c9c84ce4d7e7af3f6">BlockSolver&lt;Traits&gt;::BlockSolver</a>(<a class="code" href="classg2o_1_1_block_solver.html#a717fa8cb1dd5a212e41d8ebef67955e6">LinearSolverType</a>* linearSolver) :</div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;  <a class="code" href="classg2o_1_1_block_solver_base.html">BlockSolverBase</a>(),</div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;  _linearSolver(linearSolver)</div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;{</div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;  <span class="comment">// workspace</span></div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>=0;</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>=0;</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>=0;</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a> = 0;</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a> = 0;</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>=0;</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>=0;</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>=0;</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a> = 0;</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;  <a class="code" href="classg2o_1_1_solver.html#abcf7731347f14915bd9ba963021ea830">_xSize</a>=0;</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>=0;</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>=0;</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>=0;</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>=0;</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>=<span class="keyword">true</span>;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;}</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00065"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a0075af2df18364cf99fd80f813b8ce4b">   65</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classg2o_1_1_block_solver.html#a0075af2df18364cf99fd80f813b8ce4b">BlockSolver&lt;Traits&gt;::resize</a>(<span class="keywordtype">int</span>* blockPoseIndices, <span class="keywordtype">int</span> numPoseBlocks, </div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;              <span class="keywordtype">int</span>* blockLandmarkIndices, <span class="keywordtype">int</span> numLandmarkBlocks,</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;              <span class="keywordtype">int</span> s)</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;{</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a1877467844b7b9ab51bd6600e3a93eb0">deallocate</a>();</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;  <a class="code" href="classg2o_1_1_solver.html#ad1f85839e85f3e2c49112fb7e2b843ad">resizeVector</a>(s);</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>) {</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;    <span class="comment">// the following two are only used in schur</span></div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;    assert(<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a> &gt; 0 &amp;&amp; <span class="stringliteral">&quot;allocating with wrong size&quot;</span>);</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a> = <span class="keyword">new</span> <span class="keywordtype">double</span> [s];</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a> = <span class="keyword">new</span> <span class="keywordtype">double</span>[<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>];</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;  }</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>=<span class="keyword">new</span> <a class="code" href="classg2o_1_1_block_solver.html#a0e7f862860a1e3391cec3cfaf69c48be">PoseHessianType</a>(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>) {</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>=<span class="keyword">new</span> <a class="code" href="classg2o_1_1_block_solver.html#a0e7f862860a1e3391cec3cfaf69c48be">PoseHessianType</a>(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>=<span class="keyword">new</span> <a class="code" href="classg2o_1_1_block_solver.html#a465b1252905d90fd69b4243716620c45">LandmarkHessianType</a>(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a> = <span class="keyword">new</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_diagonal.html">SparseBlockMatrixDiagonal&lt;LandmarkMatrixType&gt;</a>(<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a1dc1115273da4863f4625b1dc9569d89">colBlockIndices</a>());</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>=<span class="keyword">new</span> <a class="code" href="classg2o_1_1_block_solver.html#aed8b44e394d2f19ca03c87adf90cc97c">PoseLandmarkHessianType</a>(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a> = <span class="keyword">new</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseLandmarkMatrixType&gt;</a>(_Hpl-&gt;rowBlockIndices(), _Hpl-&gt;colBlockIndices());</div><div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a> = <span class="keyword">new</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseMatrixType&gt;</a>(<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a1dc1115273da4863f4625b1dc9569d89">colBlockIndices</a>(), <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a4fff500693b46e4f62a2f828934d198c">rowBlockIndices</a>());</div><div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;<span class="preprocessor">#ifdef G2O_OPENMP</span></div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;    _coefficientsMutex.resize(numPoseBlocks);</div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;<span class="preprocessor">#endif</span></div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;  }</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;}</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00095"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a1877467844b7b9ab51bd6600e3a93eb0">   95</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classg2o_1_1_block_solver.html#a1877467844b7b9ab51bd6600e3a93eb0">BlockSolver&lt;Traits&gt;::deallocate</a>()</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;{</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>){</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>;</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>=0;</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;  }</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>){</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>;</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>=0;</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;  }</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>){</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>;</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a> = 0;</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;  }</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>){</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>;</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>=0;</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;  }</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>){</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>;</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>=0;</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;  }</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>) {</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;    <span class="keyword">delete</span>[] <a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>;</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a> = 0;</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;  }</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a>) {</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;    <span class="keyword">delete</span>[] <a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a>;</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a> = 0;</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;  }</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>) {</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>;</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a> = 0;</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;  }</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a>) {</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;    <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a>;</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a> = 0;</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;  }</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;}</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00136"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a7587b13ec3494c00ad95a3d0ce7fd9a9">  136</a></span>&#160;<a class="code" href="classg2o_1_1_block_solver.html#a7587b13ec3494c00ad95a3d0ce7fd9a9">BlockSolver&lt;Traits&gt;::~BlockSolver</a>()</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;{</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;  <span class="keyword">delete</span> <a class="code" href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">_linearSolver</a>;</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a1877467844b7b9ab51bd6600e3a93eb0">deallocate</a>();</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;}</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00143"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a17e4392d3cca9a9d7cf38bb46d073b86">  143</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#a17e4392d3cca9a9d7cf38bb46d073b86">BlockSolver&lt;Traits&gt;::buildStructure</a>(<span class="keywordtype">bool</span> zeroBlocks)</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;{</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;  assert(<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>);</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;  <span class="keywordtype">size_t</span> sparseDim = 0;</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>=0;</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>=0;</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>=0;</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>=0;</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;  <span class="keywordtype">int</span>* blockPoseIndices = <span class="keyword">new</span> <span class="keywordtype">int</span>[<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size()];</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;  <span class="keywordtype">int</span>* blockLandmarkIndices = <span class="keyword">new</span> <span class="keywordtype">int</span>[<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size()];</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size(); ++i) {</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v = <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>()[i];</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;    <span class="keywordtype">int</span> dim = v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a86feed80301950b100142575b48e4fca">dimension</a>();</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;    <span class="keywordflow">if</span> (! v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;      v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#aa605891d46b99ed97accf659f10bc8d5">setColInHessian</a>(<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>);</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>+=dim;</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;      blockPoseIndices[<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>]=<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;      ++<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>;</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;    } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;      v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#aa605891d46b99ed97accf659f10bc8d5">setColInHessian</a>(<a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>);</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>+=dim;</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;      blockLandmarkIndices[<a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>]=<a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>;</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;      ++<a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>;</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;    }</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;    sparseDim += dim;</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;  }</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a0075af2df18364cf99fd80f813b8ce4b">resize</a>(blockPoseIndices, <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>, blockLandmarkIndices, <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>, sparseDim);</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;  <span class="keyword">delete</span>[] blockLandmarkIndices;</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;  <span class="keyword">delete</span>[] blockPoseIndices;</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;  <span class="comment">// allocate the diagonal on Hpp and Hll</span></div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;  <span class="keywordtype">int</span> poseIdx = 0;</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;  <span class="keywordtype">int</span> landmarkIdx = 0;</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size(); ++i) {</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v = <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>()[i];</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;    <span class="keywordflow">if</span> (! v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;      <span class="comment">//assert(poseIdx == v-&gt;hessianIndex());</span></div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(poseIdx, poseIdx, <span class="keyword">true</span>);</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;      <span class="keywordflow">if</span> (zeroBlocks)</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;        m-&gt;setZero();</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;      v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a1008c0f7981a9fb11be3e3df5c4a9758">mapHessianMemory</a>(m-&gt;data());</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;      ++poseIdx;</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;    } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">LandmarkMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(landmarkIdx, landmarkIdx, <span class="keyword">true</span>);</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;      <span class="keywordflow">if</span> (zeroBlocks)</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;        m-&gt;setZero();</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;      v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a1008c0f7981a9fb11be3e3df5c4a9758">mapHessianMemory</a>(m-&gt;data());</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;      ++landmarkIdx;</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;    }</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;  }</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;  assert(poseIdx == <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a> &amp;&amp; landmarkIdx == <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>);</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;  <span class="comment">// temporary structures for building the pattern of the Schur complement</span></div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;  <a class="code" href="classg2o_1_1_sparse_block_matrix_hash_map.html">SparseBlockMatrixHashMap&lt;PoseMatrixType&gt;</a>* schurMatrixLookup = 0;</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>) {</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;    schurMatrixLookup = <span class="keyword">new</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_hash_map.html">SparseBlockMatrixHashMap&lt;PoseMatrixType&gt;</a>(<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a4fff500693b46e4f62a2f828934d198c">rowBlockIndices</a>(), <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a1dc1115273da4863f4625b1dc9569d89">colBlockIndices</a>());</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;    schurMatrixLookup-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_hash_map.html#a07d4746ccda502bd815a09aefe42eac6">blockCols</a>().resize(<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ab818f8923b845d08a38e8de1145fbb65">blockCols</a>().size());</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;  }</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;  <span class="comment">// here we assume that the landmark indices start after the pose ones</span></div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;  <span class="comment">// create the structure in Hpp, Hll and in Hpl</span></div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;  <span class="keywordflow">for</span> (SparseOptimizer::EdgeContainer::const_iterator it=<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a44edae09e8d4b2b1f7ed5b9d791f9459">activeEdges</a>().begin(); it!=<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a44edae09e8d4b2b1f7ed5b9d791f9459">activeEdges</a>().end(); ++it){</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html">OptimizableGraph::Edge</a>* e = *it;</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> viIdx = 0; viIdx &lt; e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#aba1717ff01f972bd39ba992c0d9d9e04">vertices</a>().size(); ++viIdx) {</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;      <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v1 = (<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*) e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#ab644c1c4e38a0112db1435fbc0233f25">vertex</a>(viIdx);</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;      <span class="keywordtype">int</span> ind1 = v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;      <span class="keywordflow">if</span> (ind1 == -1)</div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;        <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;      <span class="keywordtype">int</span> indexV1Bak = ind1;</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> vjIdx = viIdx + 1; vjIdx &lt; e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#aba1717ff01f972bd39ba992c0d9d9e04">vertices</a>().size(); ++vjIdx) {</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;        <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v2 = (<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*) e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#ab644c1c4e38a0112db1435fbc0233f25">vertex</a>(vjIdx);</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;        <span class="keywordtype">int</span> ind2 = v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;        <span class="keywordflow">if</span> (ind2 == -1)</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;          <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;        ind1 = indexV1Bak;</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;        <span class="keywordtype">bool</span> transposedBlock = ind1 &gt; ind2;</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;        <span class="keywordflow">if</span> (transposedBlock){ <span class="comment">// make sure, we allocate the upper triangle block</span></div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;          swap(ind1, ind2);</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;        }</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;        <span class="keywordflow">if</span> (! v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>() &amp;&amp; !v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;          <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(ind1, ind2, <span class="keyword">true</span>);</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;          <span class="keywordflow">if</span> (zeroBlocks)</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;            m-&gt;setZero();</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;          e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a3bd233fd552daa166039acf47b69a5a7">mapHessianMemory</a>(m-&gt;data(), viIdx, vjIdx, transposedBlock);</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;          <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>) {<span class="comment">// assume this is only needed in case we solve with the schur complement</span></div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;            schurMatrixLookup-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_hash_map.html#a08330c47b1b60bbe008e3c4ee2f5150f">addBlock</a>(ind1, ind2);</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;          }</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;        } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>() &amp;&amp; v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;          <span class="comment">// RAINER hmm.... should we ever reach this here????</span></div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;          <a class="code" href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">LandmarkMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(ind1-<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>, ind2-<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>, <span class="keyword">true</span>);</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;          <span class="keywordflow">if</span> (zeroBlocks)</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;            m-&gt;setZero();</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;          e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a3bd233fd552daa166039acf47b69a5a7">mapHessianMemory</a>(m-&gt;data(), viIdx, vjIdx, <span class="keyword">false</span>);</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;        } <span class="keywordflow">else</span> { </div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;          <span class="keywordflow">if</span> (v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){ </div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;            <a class="code" href="classg2o_1_1_block_solver.html#a96bf60b923f816086cd2f24de38736ec">PoseLandmarkMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>(),v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>()-<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>, <span class="keyword">true</span>);</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;            <span class="keywordflow">if</span> (zeroBlocks)</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;              m-&gt;setZero();</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;            e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a3bd233fd552daa166039acf47b69a5a7">mapHessianMemory</a>(m-&gt;data(), viIdx, vjIdx, <span class="keyword">true</span>); <span class="comment">// transpose the block before writing to it</span></div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;          } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;            <a class="code" href="classg2o_1_1_block_solver.html#a96bf60b923f816086cd2f24de38736ec">PoseLandmarkMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>(),v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>()-<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>, <span class="keyword">true</span>);</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;            <span class="keywordflow">if</span> (zeroBlocks)</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;              m-&gt;setZero();</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;            e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a3bd233fd552daa166039acf47b69a5a7">mapHessianMemory</a>(m-&gt;data(), viIdx, vjIdx, <span class="keyword">false</span>); <span class="comment">// directly the block</span></div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;          }</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;        }</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;      }</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;    }</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;  }</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;  <span class="keywordflow">if</span> (! <a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>)</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_diagonal.html#a9da10533727d4bd9312a591d2e1d9361">diagonal</a>().resize(landmarkIdx);</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a783a9c976391f0c698866e34eeef43f5">fillSparseBlockMatrixCCS</a>(*<a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>);</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size(); ++i) {</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v = <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>()[i];</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;    <span class="keywordflow">if</span> (v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;      <span class="keyword">const</span> <a class="code" href="classg2o_1_1_hyper_graph.html#a5e2970e236c0dcb4eff7c205d7b6b4ae">HyperGraph::EdgeSet</a>&amp; vedges=v-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_vertex.html#a3794ff3310c1dc05619bf0b77eaa4caa">edges</a>();</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;      <span class="keywordflow">for</span> (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0; i&lt;(*it1)-&gt;vertices().size(); ++i)</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;        {</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;          <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v1= (<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*) (*it1)-&gt;vertex(i);</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;          <span class="keywordflow">if</span> (v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>()==-1 || v1==v)</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;            <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;          <span class="keywordflow">for</span>  (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){</div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;            <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0; j&lt;(*it2)-&gt;vertices().size(); ++j)</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;            {</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;              <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v2= (<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*) (*it2)-&gt;vertex(j);</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;              <span class="keywordflow">if</span> (v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>()==-1 || v2==v)</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;                <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;              <span class="keywordtype">int</span> i1=v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;              <span class="keywordtype">int</span> i2=v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;              <span class="keywordflow">if</span> (i1&lt;=i2) {</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;                schurMatrixLookup-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_hash_map.html#a08330c47b1b60bbe008e3c4ee2f5150f">addBlock</a>(i1, i2);</div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;              }</div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;            }</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;          }</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;        }</div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;      }</div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;    }</div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;  }</div><div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a0b1e9dc3a24b0ab41d7002396a61c833">takePatternFromHash</a>(*schurMatrixLookup);</div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;  <span class="keyword">delete</span> schurMatrixLookup;</div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a3808d27b1def55e9dd6e26b678d8f728">fillSparseBlockMatrixCCSTransposed</a>(*<a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a>);</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;</div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;}</div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00298"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a662473598270cdf89075607f87440759">  298</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#a662473598270cdf89075607f87440759">BlockSolver&lt;Traits&gt;::updateStructure</a>(<span class="keyword">const</span> std::vector&lt;HyperGraph::Vertex*&gt;&amp; vset, <span class="keyword">const</span> <a class="code" href="classg2o_1_1_hyper_graph.html#a5e2970e236c0dcb4eff7c205d7b6b4ae">HyperGraph::EdgeSet</a>&amp; edges)</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;{</div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;  <span class="keywordflow">for</span> (std::vector&lt;HyperGraph::Vertex*&gt;::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {</div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v = <span class="keyword">static_cast&lt;</span><a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*<span class="keyword">&gt;</span>(*vit);</div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;    <span class="keywordtype">int</span> dim = v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a86feed80301950b100142575b48e4fca">dimension</a>();</div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;    <span class="keywordflow">if</span> (! v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()){</div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;      v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#aa605891d46b99ed97accf659f10bc8d5">setColInHessian</a>(<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>);</div><div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>+=dim;</div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a4fff500693b46e4f62a2f828934d198c">rowBlockIndices</a>().push_back(<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>);</div><div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a1dc1115273da4863f4625b1dc9569d89">colBlockIndices</a>().push_back(<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>);</div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ab818f8923b845d08a38e8de1145fbb65">blockCols</a>().push_back(<span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix.html">SparseBlockMatrix&lt;PoseMatrixType&gt;::IntBlockMap</a>());</div><div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;      ++<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>;</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;      <span class="keywordtype">int</span> ind = v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(ind, ind, <span class="keyword">true</span>);</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;      v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a1008c0f7981a9fb11be3e3df5c4a9758">mapHessianMemory</a>(m-&gt;data());</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;    } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;      std::cerr &lt;&lt; <span class="stringliteral">&quot;updateStructure(): Schur not supported&quot;</span> &lt;&lt; std::endl;</div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;      abort();</div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;    }</div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;  }</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;  <a class="code" href="classg2o_1_1_solver.html#ad1f85839e85f3e2c49112fb7e2b843ad">resizeVector</a>(<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a> + <a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>);</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;</div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;  <span class="keywordflow">for</span> (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {</div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html">OptimizableGraph::Edge</a>* e = <span class="keyword">static_cast&lt;</span><a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html">OptimizableGraph::Edge</a>*<span class="keyword">&gt;</span>(*it);</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> viIdx = 0; viIdx &lt; e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#aba1717ff01f972bd39ba992c0d9d9e04">vertices</a>().size(); ++viIdx) {</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;      <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v1 = (<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*) e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#ab644c1c4e38a0112db1435fbc0233f25">vertex</a>(viIdx);</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;      <span class="keywordtype">int</span> ind1 = v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;      <span class="keywordtype">int</span> indexV1Bak = ind1;</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;      <span class="keywordflow">if</span> (ind1 == -1)</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;        <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> vjIdx = viIdx + 1; vjIdx &lt; e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#aba1717ff01f972bd39ba992c0d9d9e04">vertices</a>().size(); ++vjIdx) {</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;        <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v2 = (<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*) e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#ab644c1c4e38a0112db1435fbc0233f25">vertex</a>(vjIdx);</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;        <span class="keywordtype">int</span> ind2 = v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">hessianIndex</a>();</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;        <span class="keywordflow">if</span> (ind2 == -1)</div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;          <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;        ind1 = indexV1Bak;</div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;        <span class="keywordtype">bool</span> transposedBlock = ind1 &gt; ind2;</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;        <span class="keywordflow">if</span> (transposedBlock) <span class="comment">// make sure, we allocate the upper triangular block</span></div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;          swap(ind1, ind2);</div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;        <span class="keywordflow">if</span> (! v1-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>() &amp;&amp; !v2-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>()) {</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;          <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a>* m = <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(ind1, ind2, <span class="keyword">true</span>);</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;          e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a3bd233fd552daa166039acf47b69a5a7">mapHessianMemory</a>(m-&gt;data(), viIdx, vjIdx, transposedBlock);</div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;        } <span class="keywordflow">else</span> { </div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;          std::cerr &lt;&lt; <a class="code" href="macros_8h.html#a9c15fe1e91b07ea3280f5239f9841b67">__PRETTY_FUNCTION__</a> &lt;&lt; <span class="stringliteral">&quot;: not supported&quot;</span> &lt;&lt; std::endl;</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;        }</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;      }</div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;    }</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;</div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;  }</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;</div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;}</div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;</div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00354"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a589a75a131cce100c1945ad2786214d7">  354</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#a589a75a131cce100c1945ad2786214d7">BlockSolver&lt;Traits&gt;::solve</a>(){</div><div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;  <span class="comment">//cerr &lt;&lt; __PRETTY_FUNCTION__ &lt;&lt; endl;</span></div><div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;  <span class="keywordflow">if</span> (! <a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>){</div><div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;    <span class="keywordtype">double</span> t=<a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>();</div><div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;    <span class="keywordtype">bool</span> ok = <a class="code" href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">_linearSolver</a>-&gt;<a class="code" href="classg2o_1_1_linear_solver.html#aa44b40826d50203c8ce2ff258c34e030">solve</a>(*<a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>, <a class="code" href="classg2o_1_1_solver.html#a94ee5e303a754f4ff338a7b032c214ae">_x</a>, <a class="code" href="classg2o_1_1_solver.html#a52c92c9bf5db0da3322da3a02dbeb245">_b</a>);</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;    <a class="code" href="structg2o_1_1_g2_o_batch_statistics.html">G2OBatchStatistics</a>* globalStats = <a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a06f9c667d8b48b8f0023e27011818b64">G2OBatchStatistics::globalStats</a>();</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;    <span class="keywordflow">if</span> (globalStats) {</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;      globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#adb423c3dac9262b7f4a315bbb108464a">timeLinearSolver</a> = <a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>() - t;</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;      globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a618a15ba153da1a99b19a2d779cf3764">hessianDimension</a> = globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#ae2af64720e3f99557924b9cf92ab6f20">hessianPoseDimension</a> = <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ac9dfcc991976a6c21dcbed961f06282d">cols</a>();</div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;    }</div><div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;    <span class="keywordflow">return</span> ok;</div><div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;  }</div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;</div><div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;  <span class="comment">// schur thing</span></div><div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;</div><div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;  <span class="comment">// backup the coefficient matrix</span></div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;  <span class="keywordtype">double</span> t=<a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>();</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;  <span class="comment">// _Hschur = _Hpp, but keeping the pattern of _Hschur</span></div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ae873eb42e1b3fbbe44d3cee48959c09a">add</a>(<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>);</div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;</div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;  <span class="comment">//_DInvSchur-&gt;clear();</span></div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;  memset (<a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>, 0, <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));</div><div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;<span class="preprocessor"># ifdef G2O_OPENMP</span></div><div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;<span class="preprocessor"># pragma omp parallel for default (shared) schedule(dynamic, 10)</span></div><div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;<span class="preprocessor"># endif</span></div><div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> landmarkIndex = 0; landmarkIndex &lt; static_cast&lt;int&gt;(<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ab818f8923b845d08a38e8de1145fbb65">blockCols</a>().size()); ++landmarkIndex) {</div><div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;    <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix.html">SparseBlockMatrix&lt;LandmarkMatrixType&gt;::IntBlockMap</a>&amp; marginalizeColumn = <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ab818f8923b845d08a38e8de1145fbb65">blockCols</a>()[landmarkIndex];</div><div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160;    assert(marginalizeColumn.size() == 1 &amp;&amp; <span class="stringliteral">&quot;more than one block in _Hll column&quot;</span>);</div><div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;</div><div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;    <span class="comment">// calculate inverse block for the landmark</span></div><div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;    <span class="keyword">const</span> <a class="code" href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">LandmarkMatrixType</a> * D = marginalizeColumn.begin()-&gt;second;</div><div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;    assert (D &amp;&amp; D-&gt;rows()==D-&gt;cols() &amp;&amp; <span class="stringliteral">&quot;Error in landmark matrix&quot;</span>);</div><div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">LandmarkMatrixType</a>&amp; Dinv = <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_diagonal.html#a9da10533727d4bd9312a591d2e1d9361">diagonal</a>()[landmarkIndex];</div><div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;    Dinv = D-&gt;inverse();</div><div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;</div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a19ade5e432f32e46557192ae75074304">LandmarkVectorType</a>  db(D-&gt;rows());</div><div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> j=0; j&lt;D-&gt;rows(); ++j) {</div><div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;      db[j]=<a class="code" href="classg2o_1_1_solver.html#a52c92c9bf5db0da3322da3a02dbeb245">_b</a>[<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af718590d044a1b44e4079136c29efcd1">rowBaseOfBlock</a>(landmarkIndex) + <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a> + j];</div><div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;    }</div><div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;    db=Dinv*db;</div><div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;</div><div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;    assert((<span class="keywordtype">size_t</span>)landmarkIndex &lt; _HplCCS-&gt;blockCols().size() &amp;&amp; <span class="stringliteral">&quot;Index out of bounds&quot;</span>);</div><div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;    <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseLandmarkMatrixType&gt;::SparseColumn</a>&amp; landmarkColumn = <a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#a30586863195a37c6ac87d7fb17376ee7">blockCols</a>()[landmarkIndex];</div><div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;</div><div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseLandmarkMatrixType&gt;::SparseColumn::const_iterator</a> it_outer = landmarkColumn.begin();</div><div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;        it_outer != landmarkColumn.end(); ++it_outer) {</div><div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;      <span class="keywordtype">int</span> i1 = it_outer-&gt;row;</div><div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;</div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;      <span class="keyword">const</span> <a class="code" href="classg2o_1_1_block_solver.html#a96bf60b923f816086cd2f24de38736ec">PoseLandmarkMatrixType</a>* Bi = it_outer-&gt;block;</div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;      assert(Bi);</div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;</div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a96bf60b923f816086cd2f24de38736ec">PoseLandmarkMatrixType</a> BDinv = (*Bi)*(Dinv);</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;      assert(<a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#aa687d275090226a00f2685d66d134fcd">rowBaseOfBlock</a>(i1) &lt; <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a> &amp;&amp; <span class="stringliteral">&quot;Index out of bounds&quot;</span>);</div><div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;      <span class="keyword">typename</span> PoseVectorType::MapType Bb(&amp;<a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>[<a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#aa687d275090226a00f2685d66d134fcd">rowBaseOfBlock</a>(i1)], Bi-&gt;rows());</div><div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;<span class="preprocessor">#    ifdef G2O_OPENMP</span></div><div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;      <a class="code" href="classg2o_1_1_scoped_open_m_p_mutex.html">ScopedOpenMPMutex</a> mutexLock(&amp;_coefficientsMutex[i1]);</div><div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;<span class="preprocessor">#    endif</span></div><div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;      Bb.noalias() += (*Bi)*db;</div><div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;</div><div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;      assert(i1 &gt;= 0 &amp;&amp; i1 &lt; static_cast&lt;int&gt;(<a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#a30586863195a37c6ac87d7fb17376ee7">blockCols</a>().size()) &amp;&amp; <span class="stringliteral">&quot;Index out of bounds&quot;</span>);</div><div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;      <span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseMatrixType&gt;::SparseColumn::iterator</a> targetColumnIt = <a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#a30586863195a37c6ac87d7fb17376ee7">blockCols</a>()[i1].begin();</div><div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;</div><div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;      <span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseLandmarkMatrixType&gt;::RowBlock</a> aux(i1, 0);</div><div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;      <span class="keyword">typename</span> <a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html">SparseBlockMatrixCCS&lt;PoseLandmarkMatrixType&gt;::SparseColumn::const_iterator</a> it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);</div><div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;      <span class="keywordflow">for</span> (; it_inner != landmarkColumn.end(); ++it_inner) {</div><div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;        <span class="keywordtype">int</span> i2 = it_inner-&gt;row;</div><div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;        <span class="keyword">const</span> <a class="code" href="classg2o_1_1_block_solver.html#a96bf60b923f816086cd2f24de38736ec">PoseLandmarkMatrixType</a>* Bj = it_inner-&gt;block;</div><div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160;        assert(Bj); </div><div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;        <span class="keywordflow">while</span> (targetColumnIt-&gt;row &lt; i2 <span class="comment">/*&amp;&amp; targetColumnIt != _HschurTransposedCCS-&gt;blockCols()[i1].end()*/</span>)</div><div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;          ++targetColumnIt;</div><div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;        assert(targetColumnIt != <a class="code" href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">_HschurTransposedCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#a30586863195a37c6ac87d7fb17376ee7">blockCols</a>()[i1].end() &amp;&amp; targetColumnIt-&gt;row == i2 &amp;&amp; <span class="stringliteral">&quot;invalid iterator, something wrong with the matrix structure&quot;</span>);</div><div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;        <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a>* Hi1i2 = targetColumnIt-&gt;block;<span class="comment">//_Hschur-&gt;block(i1,i2);</span></div><div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;        assert(Hi1i2);</div><div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;        (*Hi1i2).noalias() -= BDinv*Bj-&gt;transpose();</div><div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;      }</div><div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;    }</div><div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;  }</div><div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160;  <span class="comment">//cerr &lt;&lt; &quot;Solve [marginalize] = &quot; &lt;&lt;  get_monotonic_time()-t &lt;&lt; endl;</span></div><div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;</div><div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;  <span class="comment">// _bschur = _b for calling solver, and not touching _b</span></div><div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;  memcpy(<a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a>, <a class="code" href="classg2o_1_1_solver.html#a52c92c9bf5db0da3322da3a02dbeb245">_b</a>, <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a> * <span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));</div><div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>; ++i){</div><div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a>[i]-=<a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>[i];</div><div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;  }</div><div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;</div><div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;  <a class="code" href="structg2o_1_1_g2_o_batch_statistics.html">G2OBatchStatistics</a>* globalStats = <a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a06f9c667d8b48b8f0023e27011818b64">G2OBatchStatistics::globalStats</a>();</div><div class="line"><a name="l00442"></a><span class="lineno">  442</span>&#160;  <span class="keywordflow">if</span> (globalStats){</div><div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;    globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a79d8935c9af46d716ef2b85b777fabe1">timeSchurComplement</a> = <a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>() - t;</div><div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;  }</div><div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;</div><div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;  t=<a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>();</div><div class="line"><a name="l00447"></a><span class="lineno">  447</span>&#160;  <span class="keywordtype">bool</span> solvedPoses = <a class="code" href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">_linearSolver</a>-&gt;<a class="code" href="classg2o_1_1_linear_solver.html#aa44b40826d50203c8ce2ff258c34e030">solve</a>(*<a class="code" href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">_Hschur</a>, <a class="code" href="classg2o_1_1_solver.html#a94ee5e303a754f4ff338a7b032c214ae">_x</a>, <a class="code" href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">_bschur</a>);</div><div class="line"><a name="l00448"></a><span class="lineno">  448</span>&#160;  <span class="keywordflow">if</span> (globalStats) {</div><div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;    globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#adb423c3dac9262b7f4a315bbb108464a">timeLinearSolver</a> = <a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>() - t;</div><div class="line"><a name="l00450"></a><span class="lineno">  450</span>&#160;    globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#ae2af64720e3f99557924b9cf92ab6f20">hessianPoseDimension</a> = <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ac9dfcc991976a6c21dcbed961f06282d">cols</a>();</div><div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;    globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a0cfa7018402074f47defb9cd5d9d4b64">hessianLandmarkDimension</a> = <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#ac9dfcc991976a6c21dcbed961f06282d">cols</a>();</div><div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;    globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a618a15ba153da1a99b19a2d779cf3764">hessianDimension</a> = globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#ae2af64720e3f99557924b9cf92ab6f20">hessianPoseDimension</a> + globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a0cfa7018402074f47defb9cd5d9d4b64">hessianLandmarkDimension</a>;</div><div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;  }</div><div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;  <span class="comment">//cerr &lt;&lt; &quot;Solve [decompose and solve] = &quot; &lt;&lt;  get_monotonic_time()-t &lt;&lt; endl;</span></div><div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;</div><div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;  <span class="keywordflow">if</span> (! solvedPoses)</div><div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;</div><div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;  <span class="comment">// _x contains the solution for the poses, now applying it to the landmarks to get the new part of the</span></div><div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;  <span class="comment">// solution;</span></div><div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;  <span class="keywordtype">double</span>* xp = <a class="code" href="classg2o_1_1_solver.html#a94ee5e303a754f4ff338a7b032c214ae">_x</a>;</div><div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;  <span class="keywordtype">double</span>* cp = <a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a>;</div><div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;</div><div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;  <span class="keywordtype">double</span>* xl=<a class="code" href="classg2o_1_1_solver.html#a94ee5e303a754f4ff338a7b032c214ae">_x</a>+<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>;</div><div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;  <span class="keywordtype">double</span>* cl=<a class="code" href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">_coefficients</a> + <a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>;</div><div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;  <span class="keywordtype">double</span>* bl=<a class="code" href="classg2o_1_1_solver.html#a52c92c9bf5db0da3322da3a02dbeb245">_b</a>+<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>;</div><div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;</div><div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;  <span class="comment">// cp = -xp</span></div><div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>; ++i)</div><div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;    cp[i]=-xp[i];</div><div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;</div><div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;  <span class="comment">// cl = bl</span></div><div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;  memcpy(cl,bl,<a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));</div><div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;</div><div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160;  <span class="comment">// cl = bl - Bt * xp</span></div><div class="line"><a name="l00476"></a><span class="lineno">  476</span>&#160;  <span class="comment">//Bt-&gt;multiply(cl, cp);</span></div><div class="line"><a name="l00477"></a><span class="lineno">  477</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">_HplCCS</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_c_c_s.html#ade50ae61556e057549a64962b55ec194">rightMultiply</a>(cl, cp);</div><div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;</div><div class="line"><a name="l00479"></a><span class="lineno">  479</span>&#160;  <span class="comment">// xl = Dinv * cl</span></div><div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;  memset(xl,0, <a class="code" href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">_sizeLandmarks</a>*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));</div><div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">_DInvSchur</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix_diagonal.html#ae7d969c12506cdcd1b7d54128f3b6985">multiply</a>(xl,cl);</div><div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;  <span class="comment">//_DInvSchur-&gt;rightMultiply(xl,cl);</span></div><div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;  <span class="comment">//cerr &lt;&lt; &quot;Solve [landmark delta] = &quot; &lt;&lt;  get_monotonic_time()-t &lt;&lt; endl;</span></div><div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;</div><div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;}</div><div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;</div><div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;</div><div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00490"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#ac21cd7e2c9b8a1414f7a2dccb0d30a0e">  490</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#ac21cd7e2c9b8a1414f7a2dccb0d30a0e">BlockSolver&lt;Traits&gt;::computeMarginals</a>(<a class="code" href="classg2o_1_1_sparse_block_matrix.html">SparseBlockMatrix&lt;MatrixXd&gt;</a>&amp; spinv, <span class="keyword">const</span> std::vector&lt;std::pair&lt;int, int&gt; &gt;&amp; blockIndices)</div><div class="line"><a name="l00491"></a><span class="lineno">  491</span>&#160;{</div><div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;  <span class="keywordtype">double</span> t = <a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>();</div><div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;  <span class="keywordtype">bool</span> ok = <a class="code" href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">_linearSolver</a>-&gt;<a class="code" href="classg2o_1_1_linear_solver.html#adc74484f72bbe373622581fd597c1be3">solvePattern</a>(spinv, blockIndices, *<a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>);</div><div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;  <a class="code" href="structg2o_1_1_g2_o_batch_statistics.html">G2OBatchStatistics</a>* globalStats = <a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a06f9c667d8b48b8f0023e27011818b64">G2OBatchStatistics::globalStats</a>();</div><div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;  <span class="keywordflow">if</span> (globalStats) {</div><div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;    globalStats-&gt;<a class="code" href="structg2o_1_1_g2_o_batch_statistics.html#a58e5d0960dc35e682424dce2b50e4fa9">timeMarginals</a> = <a class="code" href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">get_monotonic_time</a>() - t;</div><div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;  }</div><div class="line"><a name="l00498"></a><span class="lineno">  498</span>&#160;  <span class="keywordflow">return</span> ok;</div><div class="line"><a name="l00499"></a><span class="lineno">  499</span>&#160;}</div><div class="line"><a name="l00500"></a><span class="lineno">  500</span>&#160;</div><div class="line"><a name="l00501"></a><span class="lineno">  501</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00502"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a2654a8d52f38e5ce23720a8de302e2e7">  502</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#a2654a8d52f38e5ce23720a8de302e2e7">BlockSolver&lt;Traits&gt;::buildSystem</a>()</div><div class="line"><a name="l00503"></a><span class="lineno">  503</span>&#160;{</div><div class="line"><a name="l00504"></a><span class="lineno">  504</span>&#160;  <span class="comment">// clear b vector</span></div><div class="line"><a name="l00505"></a><span class="lineno">  505</span>&#160;<span class="preprocessor"># ifdef G2O_OPENMP</span></div><div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;<span class="preprocessor"># pragma omp parallel for default (shared) if (_optimizer-&gt;indexMapping().size() &gt; 1000)</span></div><div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;<span class="preprocessor"># endif</span></div><div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; static_cast&lt;int&gt;(<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size()); ++i) {</div><div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v=<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>()[i];</div><div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;    assert(v);</div><div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;    v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a803897f6bae25dece4d7e23330f0f9da">clearQuadraticForm</a>();</div><div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;  }</div><div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">_doSchur</a>) {</div><div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;  }</div><div class="line"><a name="l00518"></a><span class="lineno">  518</span>&#160;</div><div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;  <span class="comment">// resetting the terms for the pairwise constraints</span></div><div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;  <span class="comment">// built up the current system by storing the Hessian blocks in the edges and vertices</span></div><div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;<span class="preprocessor"># ifndef G2O_OPENMP</span></div><div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;  <span class="comment">// no threading, we do not need to copy the workspace</span></div><div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;  <a class="code" href="classg2o_1_1_jacobian_workspace.html">JacobianWorkspace</a>&amp; jacobianWorkspace = <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="structg2o_1_1_optimizable_graph.html#aa669dbd1d6e34e49fecda711ff1b78c6">jacobianWorkspace</a>();</div><div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;<span class="preprocessor"># else</span></div><div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;  <span class="comment">// if running with threads need to produce copies of the workspace for each thread</span></div><div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;  <a class="code" href="classg2o_1_1_jacobian_workspace.html">JacobianWorkspace</a> jacobianWorkspace = <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="structg2o_1_1_optimizable_graph.html#aa669dbd1d6e34e49fecda711ff1b78c6">jacobianWorkspace</a>();</div><div class="line"><a name="l00527"></a><span class="lineno">  527</span>&#160;<span class="preprocessor"># pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer-&gt;activeEdges().size() &gt; 100)</span></div><div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;<span class="preprocessor"># endif</span></div><div class="line"><a name="l00529"></a><span class="lineno">  529</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> k = 0; k &lt; static_cast&lt;int&gt;(<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a44edae09e8d4b2b1f7ed5b9d791f9459">activeEdges</a>().size()); ++k) {</div><div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html">OptimizableGraph::Edge</a>* e = <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a44edae09e8d4b2b1f7ed5b9d791f9459">activeEdges</a>()[k];</div><div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;    e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a0fdad5ebfb4efec9f893b57f67e0fbe1">linearizeOplus</a>(jacobianWorkspace); <span class="comment">// jacobian of the nodes&#39; oplus (manifold)</span></div><div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;    e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a56fbf3430ddf591e3c619bdd1b7e4499">constructQuadraticForm</a>();</div><div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;<span class="preprocessor">#  ifndef NDEBUG</span></div><div class="line"><a name="l00534"></a><span class="lineno">  534</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#aba1717ff01f972bd39ba992c0d9d9e04">vertices</a>().size(); ++i) {</div><div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;      <span class="keyword">const</span> <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v = <span class="keyword">static_cast&lt;</span><span class="keyword">const </span><a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>*<span class="keyword">&gt;</span>(e-&gt;<a class="code" href="classg2o_1_1_hyper_graph_1_1_edge.html#ab644c1c4e38a0112db1435fbc0233f25">vertex</a>(i));</div><div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;      <span class="keywordflow">if</span> (! v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a97294e6a1a573a44fe82ba36fc52c619">fixed</a>()) {</div><div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;        <span class="keywordtype">bool</span> hasANan = <a class="code" href="namespaceg2o.html#a123840a60e0d1dde58088d15a1e1fffa">arrayHasNaN</a>(jacobianWorkspace.<a class="code" href="classg2o_1_1_jacobian_workspace.html#ad68c37d7779d3a034bc1b864cd98690b">workspaceForVertex</a>(i), e-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_edge.html#a0eb19697e201bd195c45ca78d55fd041">dimension</a>() * v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a86feed80301950b100142575b48e4fca">dimension</a>());</div><div class="line"><a name="l00538"></a><span class="lineno">  538</span>&#160;        <span class="keywordflow">if</span> (hasANan) {</div><div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;          cerr &lt;&lt; <span class="stringliteral">&quot;buildSystem(): NaN within Jacobian for edge &quot;</span> &lt;&lt; e &lt;&lt; <span class="stringliteral">&quot; for vertex &quot;</span> &lt;&lt; i &lt;&lt; endl;</div><div class="line"><a name="l00540"></a><span class="lineno">  540</span>&#160;          <span class="keywordflow">break</span>;</div><div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;        }</div><div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;      }</div><div class="line"><a name="l00543"></a><span class="lineno">  543</span>&#160;    }</div><div class="line"><a name="l00544"></a><span class="lineno">  544</span>&#160;<span class="preprocessor">#  endif</span></div><div class="line"><a name="l00545"></a><span class="lineno">  545</span>&#160;  }</div><div class="line"><a name="l00546"></a><span class="lineno">  546</span>&#160;</div><div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;  <span class="comment">// flush the current system in a sparse block matrix</span></div><div class="line"><a name="l00548"></a><span class="lineno">  548</span>&#160;<span class="preprocessor"># ifdef G2O_OPENMP</span></div><div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;<span class="preprocessor"># pragma omp parallel for default (shared) if (_optimizer-&gt;indexMapping().size() &gt; 1000)</span></div><div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;<span class="preprocessor"># endif</span></div><div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; static_cast&lt;int&gt;(<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>().size()); ++i) {</div><div class="line"><a name="l00552"></a><span class="lineno">  552</span>&#160;    <a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html">OptimizableGraph::Vertex</a>* v=<a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a>-&gt;<a class="code" href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">indexMapping</a>()[i];</div><div class="line"><a name="l00553"></a><span class="lineno">  553</span>&#160;    <span class="keywordtype">int</span> iBase = v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a4d22b2e6832ae2ec040bfc0acb4cabb9">colInHessian</a>();</div><div class="line"><a name="l00554"></a><span class="lineno">  554</span>&#160;    <span class="keywordflow">if</span> (v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">marginalized</a>())</div><div class="line"><a name="l00555"></a><span class="lineno">  555</span>&#160;      iBase+=<a class="code" href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">_sizePoses</a>;</div><div class="line"><a name="l00556"></a><span class="lineno">  556</span>&#160;    v-&gt;<a class="code" href="classg2o_1_1_optimizable_graph_1_1_vertex.html#af544f0050ea6e05950fd6e53931bdf61">copyB</a>(<a class="code" href="classg2o_1_1_solver.html#a52c92c9bf5db0da3322da3a02dbeb245">_b</a>+iBase);</div><div class="line"><a name="l00557"></a><span class="lineno">  557</span>&#160;  }</div><div class="line"><a name="l00558"></a><span class="lineno">  558</span>&#160;</div><div class="line"><a name="l00559"></a><span class="lineno">  559</span>&#160;  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00560"></a><span class="lineno">  560</span>&#160;}</div><div class="line"><a name="l00561"></a><span class="lineno">  561</span>&#160;</div><div class="line"><a name="l00562"></a><span class="lineno">  562</span>&#160;</div><div class="line"><a name="l00563"></a><span class="lineno">  563</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00564"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#acc63a23e5b35e4f72d46dc22719aa56f">  564</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#acc63a23e5b35e4f72d46dc22719aa56f">BlockSolver&lt;Traits&gt;::setLambda</a>(<span class="keywordtype">double</span> lambda, <span class="keywordtype">bool</span> backup)</div><div class="line"><a name="l00565"></a><span class="lineno">  565</span>&#160;{</div><div class="line"><a name="l00566"></a><span class="lineno">  566</span>&#160;  <span class="keywordflow">if</span> (backup) {</div><div class="line"><a name="l00567"></a><span class="lineno">  567</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a3cb6f86c522c2ea26478ad44b7c32f76">_diagonalBackupPose</a>.resize(<a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>);</div><div class="line"><a name="l00568"></a><span class="lineno">  568</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a3bc5b19faa2c45e2c04a6743b3a083de">_diagonalBackupLandmark</a>.resize(<a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>);</div><div class="line"><a name="l00569"></a><span class="lineno">  569</span>&#160;  }</div><div class="line"><a name="l00570"></a><span class="lineno">  570</span>&#160;<span class="preprocessor"># ifdef G2O_OPENMP</span></div><div class="line"><a name="l00571"></a><span class="lineno">  571</span>&#160;<span class="preprocessor"># pragma omp parallel for default (shared) if (_numPoses &gt; 100)</span></div><div class="line"><a name="l00572"></a><span class="lineno">  572</span>&#160;<span class="preprocessor"># endif</span></div><div class="line"><a name="l00573"></a><span class="lineno">  573</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>; ++i) {</div><div class="line"><a name="l00574"></a><span class="lineno">  574</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a> *<a class="code" href="classg2o_1_1_solver.html#a3212ad9d80f8f5ad15b72c2b55000095">b</a>=<a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(i,i);</div><div class="line"><a name="l00575"></a><span class="lineno">  575</span>&#160;    <span class="keywordflow">if</span> (backup)</div><div class="line"><a name="l00576"></a><span class="lineno">  576</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a3cb6f86c522c2ea26478ad44b7c32f76">_diagonalBackupPose</a>[i] = b-&gt;diagonal();</div><div class="line"><a name="l00577"></a><span class="lineno">  577</span>&#160;    b-&gt;diagonal().array() += lambda;</div><div class="line"><a name="l00578"></a><span class="lineno">  578</span>&#160;  }</div><div class="line"><a name="l00579"></a><span class="lineno">  579</span>&#160;<span class="preprocessor"># ifdef G2O_OPENMP</span></div><div class="line"><a name="l00580"></a><span class="lineno">  580</span>&#160;<span class="preprocessor"># pragma omp parallel for default (shared) if (_numLandmarks &gt; 100)</span></div><div class="line"><a name="l00581"></a><span class="lineno">  581</span>&#160;<span class="preprocessor"># endif</span></div><div class="line"><a name="l00582"></a><span class="lineno">  582</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>; ++i) {</div><div class="line"><a name="l00583"></a><span class="lineno">  583</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">LandmarkMatrixType</a> *<a class="code" href="classg2o_1_1_solver.html#a3212ad9d80f8f5ad15b72c2b55000095">b</a>=<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(i,i);</div><div class="line"><a name="l00584"></a><span class="lineno">  584</span>&#160;    <span class="keywordflow">if</span> (backup)</div><div class="line"><a name="l00585"></a><span class="lineno">  585</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a3bc5b19faa2c45e2c04a6743b3a083de">_diagonalBackupLandmark</a>[i] = b-&gt;diagonal();</div><div class="line"><a name="l00586"></a><span class="lineno">  586</span>&#160;    b-&gt;diagonal().array() += lambda;</div><div class="line"><a name="l00587"></a><span class="lineno">  587</span>&#160;  }</div><div class="line"><a name="l00588"></a><span class="lineno">  588</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00589"></a><span class="lineno">  589</span>&#160;}</div><div class="line"><a name="l00590"></a><span class="lineno">  590</span>&#160;</div><div class="line"><a name="l00591"></a><span class="lineno">  591</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00592"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a2136931d7aa2f54df5207556c4685809">  592</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classg2o_1_1_block_solver.html#a2136931d7aa2f54df5207556c4685809">BlockSolver&lt;Traits&gt;::restoreDiagonal</a>()</div><div class="line"><a name="l00593"></a><span class="lineno">  593</span>&#160;{</div><div class="line"><a name="l00594"></a><span class="lineno">  594</span>&#160;  assert((<span class="keywordtype">int</span>) <a class="code" href="classg2o_1_1_block_solver.html#a3cb6f86c522c2ea26478ad44b7c32f76">_diagonalBackupPose</a>.size() == <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a> &amp;&amp; <span class="stringliteral">&quot;Mismatch in dimensions&quot;</span>);</div><div class="line"><a name="l00595"></a><span class="lineno">  595</span>&#160;  assert((<span class="keywordtype">int</span>) <a class="code" href="classg2o_1_1_block_solver.html#a3bc5b19faa2c45e2c04a6743b3a083de">_diagonalBackupLandmark</a>.size() == <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a> &amp;&amp; <span class="stringliteral">&quot;Mismatch in dimensions&quot;</span>);</div><div class="line"><a name="l00596"></a><span class="lineno">  596</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">_numPoses</a>; ++i) {</div><div class="line"><a name="l00597"></a><span class="lineno">  597</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">PoseMatrixType</a> *<a class="code" href="classg2o_1_1_solver.html#a3212ad9d80f8f5ad15b72c2b55000095">b</a>=<a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(i,i);</div><div class="line"><a name="l00598"></a><span class="lineno">  598</span>&#160;    b-&gt;diagonal() = <a class="code" href="classg2o_1_1_block_solver.html#a3cb6f86c522c2ea26478ad44b7c32f76">_diagonalBackupPose</a>[i];</div><div class="line"><a name="l00599"></a><span class="lineno">  599</span>&#160;  }</div><div class="line"><a name="l00600"></a><span class="lineno">  600</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; <a class="code" href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">_numLandmarks</a>; ++i) {</div><div class="line"><a name="l00601"></a><span class="lineno">  601</span>&#160;    <a class="code" href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">LandmarkMatrixType</a> *<a class="code" href="classg2o_1_1_solver.html#a3212ad9d80f8f5ad15b72c2b55000095">b</a>=<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">block</a>(i,i);</div><div class="line"><a name="l00602"></a><span class="lineno">  602</span>&#160;    b-&gt;diagonal() = <a class="code" href="classg2o_1_1_block_solver.html#a3bc5b19faa2c45e2c04a6743b3a083de">_diagonalBackupLandmark</a>[i];</div><div class="line"><a name="l00603"></a><span class="lineno">  603</span>&#160;  }</div><div class="line"><a name="l00604"></a><span class="lineno">  604</span>&#160;}</div><div class="line"><a name="l00605"></a><span class="lineno">  605</span>&#160;</div><div class="line"><a name="l00606"></a><span class="lineno">  606</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00607"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a8bf01018abc3bfddfa3b29a380a1d6cb">  607</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#a8bf01018abc3bfddfa3b29a380a1d6cb">BlockSolver&lt;Traits&gt;::init</a>(<a class="code" href="classg2o_1_1_sparse_optimizer.html">SparseOptimizer</a>* <a class="code" href="classg2o_1_1_solver.html#a266bc31d289e824de6166522579ed385">optimizer</a>, <span class="keywordtype">bool</span> online)</div><div class="line"><a name="l00608"></a><span class="lineno">  608</span>&#160;{</div><div class="line"><a name="l00609"></a><span class="lineno">  609</span>&#160;  <a class="code" href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">_optimizer</a> = <a class="code" href="classg2o_1_1_solver.html#a266bc31d289e824de6166522579ed385">optimizer</a>;</div><div class="line"><a name="l00610"></a><span class="lineno">  610</span>&#160;  <span class="keywordflow">if</span> (! online) {</div><div class="line"><a name="l00611"></a><span class="lineno">  611</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>)</div><div class="line"><a name="l00612"></a><span class="lineno">  612</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00613"></a><span class="lineno">  613</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>)</div><div class="line"><a name="l00614"></a><span class="lineno">  614</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">_Hpl</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00615"></a><span class="lineno">  615</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>)</div><div class="line"><a name="l00616"></a><span class="lineno">  616</span>&#160;      <a class="code" href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">_Hll</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">clear</a>();</div><div class="line"><a name="l00617"></a><span class="lineno">  617</span>&#160;  }</div><div class="line"><a name="l00618"></a><span class="lineno">  618</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">_linearSolver</a>-&gt;<a class="code" href="classg2o_1_1_linear_solver.html#aebd961a94ef6de1bc66d2ca41dd2b17b">init</a>();</div><div class="line"><a name="l00619"></a><span class="lineno">  619</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00620"></a><span class="lineno">  620</span>&#160;}</div><div class="line"><a name="l00621"></a><span class="lineno">  621</span>&#160;</div><div class="line"><a name="l00622"></a><span class="lineno">  622</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00623"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a1bff5dc13e3408fa76c019347104acd0">  623</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classg2o_1_1_block_solver.html#a1bff5dc13e3408fa76c019347104acd0">BlockSolver&lt;Traits&gt;::setWriteDebug</a>(<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#aab81798b80dcb6c4182fa1d510914234">writeDebug</a>)</div><div class="line"><a name="l00624"></a><span class="lineno">  624</span>&#160;{</div><div class="line"><a name="l00625"></a><span class="lineno">  625</span>&#160;  <a class="code" href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">_linearSolver</a>-&gt;<a class="code" href="classg2o_1_1_linear_solver.html#a969c406ccacc38705b2a88f5ed23cb9a">setWriteDebug</a>(writeDebug);</div><div class="line"><a name="l00626"></a><span class="lineno">  626</span>&#160;}</div><div class="line"><a name="l00627"></a><span class="lineno">  627</span>&#160;</div><div class="line"><a name="l00628"></a><span class="lineno">  628</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Traits&gt;</div><div class="line"><a name="l00629"></a><span class="lineno"><a class="line" href="classg2o_1_1_block_solver.html#a51563178d31f3eae6ec61993ea069a77">  629</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="classg2o_1_1_block_solver.html#a51563178d31f3eae6ec61993ea069a77">BlockSolver&lt;Traits&gt;::saveHessian</a>(<span class="keyword">const</span> std::string&amp; fileName)<span class="keyword"> const</span></div><div class="line"><a name="l00630"></a><span class="lineno">  630</span>&#160;<span class="keyword"></span>{</div><div class="line"><a name="l00631"></a><span class="lineno">  631</span>&#160;  <span class="keywordflow">return</span> <a class="code" href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">_Hpp</a>-&gt;<a class="code" href="classg2o_1_1_sparse_block_matrix.html#a21725f29b3f1bbc4e4ba77ee3f960ddc">writeOctave</a>(fileName.c_str(), <span class="keyword">true</span>);</div><div class="line"><a name="l00632"></a><span class="lineno">  632</span>&#160;}</div><div class="line"><a name="l00633"></a><span class="lineno">  633</span>&#160;</div><div class="line"><a name="l00634"></a><span class="lineno">  634</span>&#160;} <span class="comment">// end namespace</span></div><div class="ttc" id="classg2o_1_1_block_solver_html_a676a4ef473ccaecb23050284e19659af"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a676a4ef473ccaecb23050284e19659af">g2o::BlockSolver::_linearSolver</a></div><div class="ttdeci">LinearSolver&lt; PoseMatrixType &gt; * _linearSolver</div><div class="ttdef"><b>Definition:</b> block_solver.h:160</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a3cb6f86c522c2ea26478ad44b7c32f76"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a3cb6f86c522c2ea26478ad44b7c32f76">g2o::BlockSolver::_diagonalBackupPose</a></div><div class="ttdeci">std::vector&lt; PoseVectorType, Eigen::aligned_allocator&lt; PoseVectorType &gt; &gt; _diagonalBackupPose</div><div class="ttdef"><b>Definition:</b> block_solver.h:162</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a4d22b2e6832ae2ec040bfc0acb4cabb9"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a4d22b2e6832ae2ec040bfc0acb4cabb9">g2o::OptimizableGraph::Vertex::colInHessian</a></div><div class="ttdeci">int colInHessian() const</div><div class="ttdoc">get the row of this vertex in the Hessian </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:323</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_a3212ad9d80f8f5ad15b72c2b55000095"><div class="ttname"><a href="classg2o_1_1_solver.html#a3212ad9d80f8f5ad15b72c2b55000095">g2o::Solver::b</a></div><div class="ttdeci">double * b()</div><div class="ttdoc">return b, the right hand side of the system </div><div class="ttdef"><b>Definition:</b> solver.h:98</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_aafddeb1d0a4218fc9c3c77169e20f81a"><div class="ttname"><a href="classg2o_1_1_block_solver.html#aafddeb1d0a4218fc9c3c77169e20f81a">g2o::BlockSolver::_bschur</a></div><div class="ttdeci">double * _bschur</div><div class="ttdef"><b>Definition:</b> block_solver.h:172</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_a266bc31d289e824de6166522579ed385"><div class="ttname"><a href="classg2o_1_1_solver.html#a266bc31d289e824de6166522579ed385">g2o::Solver::optimizer</a></div><div class="ttdeci">SparseOptimizer * optimizer() const</div><div class="ttdoc">the optimizer (graph) on which the solver works </div><div class="ttdef"><b>Definition:</b> solver.h:105</div></div>
<div class="ttc" id="classg2o_1_1_jacobian_workspace_html_ad68c37d7779d3a034bc1b864cd98690b"><div class="ttname"><a href="classg2o_1_1_jacobian_workspace.html#ad68c37d7779d3a034bc1b864cd98690b">g2o::JacobianWorkspace::workspaceForVertex</a></div><div class="ttdeci">double * workspaceForVertex(int vertexIndex)</div><div class="ttdoc">return the workspace for a vertex in an edge </div><div class="ttdef"><b>Definition:</b> jacobian_workspace.h:82</div></div>
<div class="ttc" id="macros_8h_html_a9c15fe1e91b07ea3280f5239f9841b67"><div class="ttname"><a href="macros_8h.html#a9c15fe1e91b07ea3280f5239f9841b67">__PRETTY_FUNCTION__</a></div><div class="ttdeci">#define __PRETTY_FUNCTION__</div><div class="ttdef"><b>Definition:</b> macros.h:95</div></div>
<div class="ttc" id="classg2o_1_1_sparse_optimizer_html_a44edae09e8d4b2b1f7ed5b9d791f9459"><div class="ttname"><a href="classg2o_1_1_sparse_optimizer.html#a44edae09e8d4b2b1f7ed5b9d791f9459">g2o::SparseOptimizer::activeEdges</a></div><div class="ttdeci">const EdgeContainer &amp; activeEdges() const</div><div class="ttdoc">the edges active in the current optimization </div><div class="ttdef"><b>Definition:</b> sparse_optimizer.h:195</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_aaca7b38d2e9a18eebf9e6f5957af0cf7"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#aaca7b38d2e9a18eebf9e6f5957af0cf7">g2o::SparseBlockMatrix::block</a></div><div class="ttdeci">SparseMatrixBlock * block(int r, int c, bool alloc=false)</div><div class="ttdoc">returns the block at location r,c. if alloc=true he block is created if it does not exist ...</div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:92</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_aff3275985d996329df15070348c21292"><div class="ttname"><a href="classg2o_1_1_solver.html#aff3275985d996329df15070348c21292">g2o::Solver::_optimizer</a></div><div class="ttdeci">SparseOptimizer * _optimizer</div><div class="ttdef"><b>Definition:</b> solver.h:135</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_ac21cd7e2c9b8a1414f7a2dccb0d30a0e"><div class="ttname"><a href="classg2o_1_1_block_solver.html#ac21cd7e2c9b8a1414f7a2dccb0d30a0e">g2o::BlockSolver::computeMarginals</a></div><div class="ttdeci">virtual bool computeMarginals(SparseBlockMatrix&lt; MatrixXd &gt; &amp;spinv, const std::vector&lt; std::pair&lt; int, int &gt; &gt; &amp;blockIndices)</div><div class="ttdoc">computes the block diagonal elements of the pattern specified in the input and stores them in given S...</div><div class="ttdef"><b>Definition:</b> block_solver.hpp:490</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html">g2o::OptimizableGraph::Vertex</a></div><div class="ttdoc">A general case Vertex for optimization. </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:127</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_diagonal_html_a9da10533727d4bd9312a591d2e1d9361"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_diagonal.html#a9da10533727d4bd9312a591d2e1d9361">g2o::SparseBlockMatrixDiagonal::diagonal</a></div><div class="ttdeci">const DiagonalVector &amp; diagonal() const</div><div class="ttdoc">the block matrices per block-column </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_diagonal.h:71</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a589a75a131cce100c1945ad2786214d7"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a589a75a131cce100c1945ad2786214d7">g2o::BlockSolver::solve</a></div><div class="ttdeci">virtual bool solve()</div><div class="ttdoc">solve Ax = b </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:354</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a1008c0f7981a9fb11be3e3df5c4a9758"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a1008c0f7981a9fb11be3e3df5c4a9758">g2o::OptimizableGraph::Vertex::mapHessianMemory</a></div><div class="ttdeci">virtual void mapHessianMemory(double *d)=0</div><div class="ttdoc">maps the internal matrix to some external memory location </div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a86feed80301950b100142575b48e4fca"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a86feed80301950b100142575b48e4fca">g2o::OptimizableGraph::Vertex::dimension</a></div><div class="ttdeci">int dimension() const</div><div class="ttdoc">dimension of the estimated state belonging to this node </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:315</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a88d4c24df24a8fb72be1a4e4cff03d71"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a88d4c24df24a8fb72be1a4e4cff03d71">g2o::BlockSolver::_Hll</a></div><div class="ttdeci">SparseBlockMatrix&lt; LandmarkMatrixType &gt; * _Hll</div><div class="ttdef"><b>Definition:</b> block_solver.h:151</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a8c7c43d361bd31e3e0353889ba703bc0"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a8c7c43d361bd31e3e0353889ba703bc0">g2o::BlockSolver::PoseMatrixType</a></div><div class="ttdeci">Traits::PoseMatrixType PoseMatrixType</div><div class="ttdef"><b>Definition:</b> block_solver.h:102</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a46977934a3e4fb0cd36bc4181ed3ec0e"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a46977934a3e4fb0cd36bc4181ed3ec0e">g2o::BlockSolver::_Hschur</a></div><div class="ttdeci">SparseBlockMatrix&lt; PoseMatrixType &gt; * _Hschur</div><div class="ttdef"><b>Definition:</b> block_solver.h:154</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a2136931d7aa2f54df5207556c4685809"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a2136931d7aa2f54df5207556c4685809">g2o::BlockSolver::restoreDiagonal</a></div><div class="ttdeci">virtual void restoreDiagonal()</div><div class="ttdoc">restore a previosly made backup of the diagonal </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:592</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_a4fff500693b46e4f62a2f828934d198c"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#a4fff500693b46e4f62a2f828934d198c">g2o::SparseBlockMatrix::rowBlockIndices</a></div><div class="ttdeci">const std::vector&lt; int &gt; &amp; rowBlockIndices() const</div><div class="ttdoc">indices of the row blocks </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.h:182</div></div>
<div class="ttc" id="classg2o_1_1_sparse_optimizer_html"><div class="ttname"><a href="classg2o_1_1_sparse_optimizer.html">g2o::SparseOptimizer</a></div><div class="ttdef"><b>Definition:</b> sparse_optimizer.h:45</div></div>
<div class="ttc" id="classg2o_1_1_scoped_open_m_p_mutex_html"><div class="ttname"><a href="classg2o_1_1_scoped_open_m_p_mutex.html">g2o::ScopedOpenMPMutex</a></div><div class="ttdoc">lock a mutex within a scope </div><div class="ttdef"><b>Definition:</b> openmp_mutex.h:84</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a2654a8d52f38e5ce23720a8de302e2e7"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a2654a8d52f38e5ce23720a8de302e2e7">g2o::BlockSolver::buildSystem</a></div><div class="ttdeci">virtual bool buildSystem()</div><div class="ttdoc">build the current system </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:502</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a51563178d31f3eae6ec61993ea069a77"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a51563178d31f3eae6ec61993ea069a77">g2o::BlockSolver::saveHessian</a></div><div class="ttdeci">virtual bool saveHessian(const std::string &amp;fileName) const</div><div class="ttdoc">write the hessian to disk using the specified file name </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:629</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_ab54eb7bb13f8b3a8a5f135a98f2050ec"><div class="ttname"><a href="classg2o_1_1_block_solver.html#ab54eb7bb13f8b3a8a5f135a98f2050ec">g2o::BlockSolver::_HplCCS</a></div><div class="ttdeci">SparseBlockMatrixCCS&lt; PoseLandmarkMatrixType &gt; * _HplCCS</div><div class="ttdef"><b>Definition:</b> block_solver.h:157</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a17e4392d3cca9a9d7cf38bb46d073b86"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a17e4392d3cca9a9d7cf38bb46d073b86">g2o::BlockSolver::buildStructure</a></div><div class="ttdeci">virtual bool buildStructure(bool zeroBlocks=false)</div><div class="ttdoc">build the structure of the system </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:143</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_a3808d27b1def55e9dd6e26b678d8f728"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#a3808d27b1def55e9dd6e26b678d8f728">g2o::SparseBlockMatrix::fillSparseBlockMatrixCCSTransposed</a></div><div class="ttdeci">int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS&lt; MatrixType &gt; &amp;blockCCS) const</div><div class="ttdoc">copy as transposed into a CCS structure </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:610</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_a618a15ba153da1a99b19a2d779cf3764"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#a618a15ba153da1a99b19a2d779cf3764">g2o::G2OBatchStatistics::hessianDimension</a></div><div class="ttdeci">size_t hessianDimension</div><div class="ttdoc">rows / cols of the Hessian </div><div class="ttdef"><b>Definition:</b> batch_stats.h:67</div></div>
<div class="ttc" id="namespacestd_html"><div class="ttname"><a href="namespacestd.html">std</a></div><div class="ttdoc">This file is part of ORB-SLAM2. </div></div>
<div class="ttc" id="classg2o_1_1_hyper_graph_1_1_vertex_html_a3794ff3310c1dc05619bf0b77eaa4caa"><div class="ttname"><a href="classg2o_1_1_hyper_graph_1_1_vertex.html#a3794ff3310c1dc05619bf0b77eaa4caa">g2o::HyperGraph::Vertex::edges</a></div><div class="ttdeci">const EdgeSet &amp; edges() const</div><div class="ttdoc">returns the set of hyper-edges that are leaving/entering in this vertex </div><div class="ttdef"><b>Definition:</b> hyper_graph.h:106</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a216d79149edcfa9f666140205159fde8"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a216d79149edcfa9f666140205159fde8">g2o::OptimizableGraph::Vertex::marginalized</a></div><div class="ttdeci">bool marginalized() const</div><div class="ttdoc">true =&gt; this node is marginalized out during the optimization </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:310</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_ab375a5fac964182442f38288bd8a103a"><div class="ttname"><a href="classg2o_1_1_block_solver.html#ab375a5fac964182442f38288bd8a103a">g2o::BlockSolver::_doSchur</a></div><div class="ttdeci">bool _doSchur</div><div class="ttdef"><b>Definition:</b> block_solver.h:169</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_ab818f8923b845d08a38e8de1145fbb65"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#ab818f8923b845d08a38e8de1145fbb65">g2o::SparseBlockMatrix::blockCols</a></div><div class="ttdeci">const std::vector&lt; IntBlockMap &gt; &amp; blockCols() const</div><div class="ttdoc">the block matrices per block-column </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.h:178</div></div>
<div class="ttc" id="classg2o_1_1_linear_solver_html_a969c406ccacc38705b2a88f5ed23cb9a"><div class="ttname"><a href="classg2o_1_1_linear_solver.html#a969c406ccacc38705b2a88f5ed23cb9a">g2o::LinearSolver::setWriteDebug</a></div><div class="ttdeci">virtual void setWriteDebug(bool)</div><div class="ttdef"><b>Definition:</b> linear_solver.h:80</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_hash_map_html"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_hash_map.html">g2o::SparseBlockMatrixHashMap</a></div><div class="ttdoc">Sparse matrix which uses blocks based on hash structures. </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_ccs.h:215</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_a79d8935c9af46d716ef2b85b777fabe1"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#a79d8935c9af46d716ef2b85b777fabe1">g2o::G2OBatchStatistics::timeSchurComplement</a></div><div class="ttdeci">double timeSchurComplement</div><div class="ttdoc">compute schur complement (0 if not done) </div><div class="ttdef"><b>Definition:</b> batch_stats.h:53</div></div>
<div class="ttc" id="namespace_eigen_html"><div class="ttname"><a href="namespace_eigen.html">Eigen</a></div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html">g2o::G2OBatchStatistics</a></div><div class="ttdoc">statistics about the optimization </div><div class="ttdef"><b>Definition:</b> batch_stats.h:39</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a416f480d4b27d7f8962ae7ae363f2e32"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a416f480d4b27d7f8962ae7ae363f2e32">g2o::BlockSolver::_coefficients</a></div><div class="ttdeci">double * _coefficients</div><div class="ttdef"><b>Definition:</b> block_solver.h:171</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a39ec000379885ce09cdd8c23ab6d4567"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a39ec000379885ce09cdd8c23ab6d4567">g2o::BlockSolver::_sizePoses</a></div><div class="ttdeci">int _sizePoses</div><div class="ttdef"><b>Definition:</b> block_solver.h:175</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_a06f9c667d8b48b8f0023e27011818b64"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#a06f9c667d8b48b8f0023e27011818b64">g2o::G2OBatchStatistics::globalStats</a></div><div class="ttdeci">static G2OBatchStatistics * globalStats()</div><div class="ttdef"><b>Definition:</b> batch_stats.h:72</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a19ade5e432f32e46557192ae75074304"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a19ade5e432f32e46557192ae75074304">g2o::BlockSolver::LandmarkVectorType</a></div><div class="ttdeci">Traits::LandmarkVectorType LandmarkVectorType</div><div class="ttdef"><b>Definition:</b> block_solver.h:106</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_edge_html_a56fbf3430ddf591e3c619bdd1b7e4499"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_edge.html#a56fbf3430ddf591e3c619bdd1b7e4499">g2o::OptimizableGraph::Edge::constructQuadraticForm</a></div><div class="ttdeci">virtual void constructQuadraticForm()=0</div><div class="ttdoc">Linearizes the constraint in the edge. </div></div>
<div class="ttc" id="namespaceg2o_html_a123840a60e0d1dde58088d15a1e1fffa"><div class="ttname"><a href="namespaceg2o.html#a123840a60e0d1dde58088d15a1e1fffa">g2o::arrayHasNaN</a></div><div class="ttdeci">bool arrayHasNaN(const double *array, int size, int *nanIndex=0)</div><div class="ttdoc">tests whether there is a NaN in the array </div><div class="ttdef"><b>Definition:</b> misc.h:177</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_diagonal_html"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_diagonal.html">g2o::SparseBlockMatrixDiagonal&lt; LandmarkMatrixType &gt;</a></div></div>
<div class="ttc" id="classg2o_1_1_hyper_graph_1_1_edge_html_aba1717ff01f972bd39ba992c0d9d9e04"><div class="ttname"><a href="classg2o_1_1_hyper_graph_1_1_edge.html#aba1717ff01f972bd39ba992c0d9d9e04">g2o::HyperGraph::Edge::vertices</a></div><div class="ttdeci">const VertexContainer &amp; vertices() const</div><div class="ttdoc">returns the vector of pointers to the vertices connected by the hyper-edge. </div><div class="ttdef"><b>Definition:</b> hyper_graph.h:132</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_hash_map_html_a07d4746ccda502bd815a09aefe42eac6"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_hash_map.html#a07d4746ccda502bd815a09aefe42eac6">g2o::SparseBlockMatrixHashMap::blockCols</a></div><div class="ttdeci">const std::vector&lt; SparseColumn &gt; &amp; blockCols() const</div><div class="ttdoc">the block matrices per block-column </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_ccs.h:245</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a709259fc290d746f4174d25410b7458a"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a709259fc290d746f4174d25410b7458a">g2o::BlockSolver::_numPoses</a></div><div class="ttdeci">int _numPoses</div><div class="ttdef"><b>Definition:</b> block_solver.h:174</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_base_html"><div class="ttname"><a href="classg2o_1_1_block_solver_base.html">g2o::BlockSolverBase</a></div><div class="ttdoc">base for the block solvers with some basic function interfaces </div><div class="ttdef"><b>Definition:</b> block_solver.h:83</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a97294e6a1a573a44fe82ba36fc52c619"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a97294e6a1a573a44fe82ba36fc52c619">g2o::OptimizableGraph::Vertex::fixed</a></div><div class="ttdeci">bool fixed() const</div><div class="ttdoc">true =&gt; this node is fixed during the optimization </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:305</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a05a371cfa6314d0f2d209289b01cc922"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a05a371cfa6314d0f2d209289b01cc922">g2o::OptimizableGraph::Vertex::hessianIndex</a></div><div class="ttdeci">int hessianIndex() const</div><div class="ttdoc">temporary index of this node in the parameter vector obtained from linearization </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:298</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_af544f0050ea6e05950fd6e53931bdf61"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#af544f0050ea6e05950fd6e53931bdf61">g2o::OptimizableGraph::Vertex::copyB</a></div><div class="ttdeci">virtual int copyB(double *b_) const =0</div><div class="ttdoc">copies the b vector in the array b_ </div></div>
<div class="ttc" id="namespaceg2o_html"><div class="ttname"><a href="namespaceg2o.html">g2o</a></div><div class="ttdef"><b>Definition:</b> base_binary_edge.h:37</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_a1dc1115273da4863f4625b1dc9569d89"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#a1dc1115273da4863f4625b1dc9569d89">g2o::SparseBlockMatrix::colBlockIndices</a></div><div class="ttdeci">const std::vector&lt; int &gt; &amp; colBlockIndices() const</div><div class="ttdoc">indices of the column blocks </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.h:186</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_c_c_s_html_ade50ae61556e057549a64962b55ec194"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_c_c_s.html#ade50ae61556e057549a64962b55ec194">g2o::SparseBlockMatrixCCS::rightMultiply</a></div><div class="ttdeci">void rightMultiply(double *&amp;dest, const double *src) const</div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_ccs.h:103</div></div>
<div class="ttc" id="sparse__optimizer_8h_html"><div class="ttname"><a href="sparse__optimizer_8h.html">sparse_optimizer.h</a></div></div>
<div class="ttc" id="classg2o_1_1_linear_solver_html_aebd961a94ef6de1bc66d2ca41dd2b17b"><div class="ttname"><a href="classg2o_1_1_linear_solver.html#aebd961a94ef6de1bc66d2ca41dd2b17b">g2o::LinearSolver::init</a></div><div class="ttdeci">virtual bool init()=0</div><div class="ttdoc">init for operating on matrices with a different non-zero pattern like before </div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_edge_html_a0eb19697e201bd195c45ca78d55fd041"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_edge.html#a0eb19697e201bd195c45ca78d55fd041">g2o::OptimizableGraph::Edge::dimension</a></div><div class="ttdeci">int dimension() const</div><div class="ttdoc">returns the dimensions of the error function </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:472</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a1bff5dc13e3408fa76c019347104acd0"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a1bff5dc13e3408fa76c019347104acd0">g2o::BlockSolver::setWriteDebug</a></div><div class="ttdeci">virtual void setWriteDebug(bool writeDebug)</div><div class="ttdoc">write debug output of the Hessian if system is not positive definite </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:623</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_a58e5d0960dc35e682424dce2b50e4fa9"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#a58e5d0960dc35e682424dce2b50e4fa9">g2o::G2OBatchStatistics::timeMarginals</a></div><div class="ttdeci">double timeMarginals</div><div class="ttdoc">computing the inverse elements (solve blocks) and thus the marginal covariances </div><div class="ttdef"><b>Definition:</b> batch_stats.h:64</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a1877467844b7b9ab51bd6600e3a93eb0"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a1877467844b7b9ab51bd6600e3a93eb0">g2o::BlockSolver::deallocate</a></div><div class="ttdeci">void deallocate()</div><div class="ttdef"><b>Definition:</b> block_solver.hpp:95</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_a0b1e9dc3a24b0ab41d7002396a61c833"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#a0b1e9dc3a24b0ab41d7002396a61c833">g2o::SparseBlockMatrix::takePatternFromHash</a></div><div class="ttdeci">void takePatternFromHash(SparseBlockMatrixHashMap&lt; MatrixType &gt; &amp;hashMatrix)</div><div class="ttdoc">take over the memory and matrix pattern from a hash matrix. </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:627</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_edge_html"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_edge.html">g2o::OptimizableGraph::Edge</a></div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:382</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_ae873eb42e1b3fbbe44d3cee48959c09a"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#ae873eb42e1b3fbbe44d3cee48959c09a">g2o::SparseBlockMatrix::add</a></div><div class="ttdeci">bool add(SparseBlockMatrix&lt; MatrixType &gt; *&amp;dest) const</div><div class="ttdoc">adds the current matrix to the destination </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:169</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_a21725f29b3f1bbc4e4ba77ee3f960ddc"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#a21725f29b3f1bbc4e4ba77ee3f960ddc">g2o::SparseBlockMatrix::writeOctave</a></div><div class="ttdeci">bool writeOctave(const char *filename, bool upperTriangle=true) const</div><div class="ttdoc">write the content of this matrix to a stream loadable by Octave </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:548</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_adb423c3dac9262b7f4a315bbb108464a"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#adb423c3dac9262b7f4a315bbb108464a">g2o::G2OBatchStatistics::timeLinearSolver</a></div><div class="ttdeci">double timeLinearSolver</div><div class="ttdoc">time for solving, excluding Schur setup </div><div class="ttdef"><b>Definition:</b> batch_stats.h:59</div></div>
<div class="ttc" id="namespaceg2o_html_aa64237935c7d8411b4369234a5f661ea"><div class="ttname"><a href="namespaceg2o.html#aa64237935c7d8411b4369234a5f661ea">g2o::get_monotonic_time</a></div><div class="ttdeci">double get_monotonic_time()</div><div class="ttdoc">return a monotonic increasing time which basically does not need to have a reference point...</div><div class="ttdef"><b>Definition:</b> timeutil.cpp:113</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_aed8b44e394d2f19ca03c87adf90cc97c"><div class="ttname"><a href="classg2o_1_1_block_solver.html#aed8b44e394d2f19ca03c87adf90cc97c">g2o::BlockSolver::PoseLandmarkHessianType</a></div><div class="ttdeci">Traits::PoseLandmarkHessianType PoseLandmarkHessianType</div><div class="ttdef"><b>Definition:</b> block_solver.h:110</div></div>
<div class="ttc" id="classg2o_1_1_linear_solver_html_adc74484f72bbe373622581fd597c1be3"><div class="ttname"><a href="classg2o_1_1_linear_solver.html#adc74484f72bbe373622581fd597c1be3">g2o::LinearSolver::solvePattern</a></div><div class="ttdeci">virtual bool solvePattern(SparseBlockMatrix&lt; MatrixXd &gt; &amp;spinv, const std::vector&lt; std::pair&lt; int, int &gt; &gt; &amp;blockIndices, const SparseBlockMatrix&lt; MatrixType &gt; &amp;A)</div><div class="ttdoc">Inverts the a block pattern of A in spinv. </div><div class="ttdef"><b>Definition:</b> linear_solver.h:71</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a465b1252905d90fd69b4243716620c45"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a465b1252905d90fd69b4243716620c45">g2o::BlockSolver::LandmarkHessianType</a></div><div class="ttdeci">Traits::LandmarkHessianType LandmarkHessianType</div><div class="ttdef"><b>Definition:</b> block_solver.h:109</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a0f6051339990e95aa587145a8a6f4f5f"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a0f6051339990e95aa587145a8a6f4f5f">g2o::BlockSolver::_Hpl</a></div><div class="ttdeci">SparseBlockMatrix&lt; PoseLandmarkMatrixType &gt; * _Hpl</div><div class="ttdef"><b>Definition:</b> block_solver.h:152</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_hash_map_html_a08330c47b1b60bbe008e3c4ee2f5150f"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_hash_map.html#a08330c47b1b60bbe008e3c4ee2f5150f">g2o::SparseBlockMatrixHashMap::addBlock</a></div><div class="ttdeci">MatrixType * addBlock(int r, int c, bool zeroBlock=false)</div><div class="ttdoc">add a block to the pattern, return a pointer to the added block </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_ccs.h:257</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a04701a223a14708c9c84ce4d7e7af3f6"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a04701a223a14708c9c84ce4d7e7af3f6">g2o::BlockSolver::BlockSolver</a></div><div class="ttdeci">BlockSolver(LinearSolverType *linearSolver)</div><div class="ttdoc">allocate a block solver ontop of the underlying linear solver. </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:42</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_ad1f85839e85f3e2c49112fb7e2b843ad"><div class="ttname"><a href="classg2o_1_1_solver.html#ad1f85839e85f3e2c49112fb7e2b843ad">g2o::Solver::resizeVector</a></div><div class="ttdeci">void resizeVector(size_t sx)</div><div class="ttdef"><b>Definition:</b> solver.cpp:46</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html">g2o::SparseBlockMatrix</a></div><div class="ttdoc">Sparse matrix which uses blocks. </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.h:62</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_c_c_s_html_a30586863195a37c6ac87d7fb17376ee7"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_c_c_s.html#a30586863195a37c6ac87d7fb17376ee7">g2o::SparseBlockMatrixCCS::blockCols</a></div><div class="ttdeci">const std::vector&lt; SparseColumn &gt; &amp; blockCols() const</div><div class="ttdoc">the block matrices per block-column </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_ccs.h:94</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_acea4b8ea8db5a29b63bea4bc568b0b26"><div class="ttname"><a href="classg2o_1_1_block_solver.html#acea4b8ea8db5a29b63bea4bc568b0b26">g2o::BlockSolver::_HschurTransposedCCS</a></div><div class="ttdeci">SparseBlockMatrixCCS&lt; PoseMatrixType &gt; * _HschurTransposedCCS</div><div class="ttdef"><b>Definition:</b> block_solver.h:158</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a0e7f862860a1e3391cec3cfaf69c48be"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a0e7f862860a1e3391cec3cfaf69c48be">g2o::BlockSolver::PoseHessianType</a></div><div class="ttdeci">Traits::PoseHessianType PoseHessianType</div><div class="ttdef"><b>Definition:</b> block_solver.h:108</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_af14b7aaa588b339f2c06793fcc0d4e09"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#af14b7aaa588b339f2c06793fcc0d4e09">g2o::SparseBlockMatrix::clear</a></div><div class="ttdeci">void clear(bool dealloc=false)</div><div class="ttdoc">this zeroes all the blocks. If dealloc=true the blocks are removed from memory </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:68</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_a0cfa7018402074f47defb9cd5d9d4b64"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#a0cfa7018402074f47defb9cd5d9d4b64">g2o::G2OBatchStatistics::hessianLandmarkDimension</a></div><div class="ttdeci">size_t hessianLandmarkDimension</div><div class="ttdoc">dimension of the landmark matrix in Schur </div><div class="ttdef"><b>Definition:</b> batch_stats.h:69</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_a783a9c976391f0c698866e34eeef43f5"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#a783a9c976391f0c698866e34eeef43f5">g2o::SparseBlockMatrix::fillSparseBlockMatrixCCS</a></div><div class="ttdeci">int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS&lt; MatrixType &gt; &amp;blockCCS) const</div><div class="ttdoc">copy into CCS structure </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.hpp:592</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a13a49b5aac8ae3b12ed0c349fc0788e7"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a13a49b5aac8ae3b12ed0c349fc0788e7">g2o::BlockSolver::_sizeLandmarks</a></div><div class="ttdeci">int _sizeLandmarks</div><div class="ttdef"><b>Definition:</b> block_solver.h:175</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_abcf7731347f14915bd9ba963021ea830"><div class="ttname"><a href="classg2o_1_1_solver.html#abcf7731347f14915bd9ba963021ea830">g2o::Solver::_xSize</a></div><div class="ttdeci">size_t _xSize</div><div class="ttdef"><b>Definition:</b> solver.h:138</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_edge_html_a3bd233fd552daa166039acf47b69a5a7"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_edge.html#a3bd233fd552daa166039acf47b69a5a7">g2o::OptimizableGraph::Edge::mapHessianMemory</a></div><div class="ttdeci">virtual void mapHessianMemory(double *d, int i, int j, bool rowMajor)=0</div><div class="ttdoc">maps the internal matrix to some external memory location, you need to provide the memory before call...</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a7587b13ec3494c00ad95a3d0ce7fd9a9"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a7587b13ec3494c00ad95a3d0ce7fd9a9">g2o::BlockSolver::~BlockSolver</a></div><div class="ttdeci">~BlockSolver()</div><div class="ttdef"><b>Definition:</b> block_solver.hpp:136</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_a94ee5e303a754f4ff338a7b032c214ae"><div class="ttname"><a href="classg2o_1_1_solver.html#a94ee5e303a754f4ff338a7b032c214ae">g2o::Solver::_x</a></div><div class="ttdeci">double * _x</div><div class="ttdef"><b>Definition:</b> solver.h:136</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_ab98231b7ca8e6d7f138c33d26c6f4326"><div class="ttname"><a href="classg2o_1_1_block_solver.html#ab98231b7ca8e6d7f138c33d26c6f4326">g2o::BlockSolver::_numLandmarks</a></div><div class="ttdeci">int _numLandmarks</div><div class="ttdef"><b>Definition:</b> block_solver.h:174</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_ac9dfcc991976a6c21dcbed961f06282d"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#ac9dfcc991976a6c21dcbed961f06282d">g2o::SparseBlockMatrix::cols</a></div><div class="ttdeci">int cols() const</div><div class="ttdoc">columns of the matrix </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.h:69</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_acc63a23e5b35e4f72d46dc22719aa56f"><div class="ttname"><a href="classg2o_1_1_block_solver.html#acc63a23e5b35e4f72d46dc22719aa56f">g2o::BlockSolver::setLambda</a></div><div class="ttdeci">virtual bool setLambda(double lambda, bool backup=false)</div><div class="ttdoc">update the system while performing Levenberg, i.e., modifying the diagonal components of A by doing +...</div><div class="ttdef"><b>Definition:</b> block_solver.hpp:564</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_a803897f6bae25dece4d7e23330f0f9da"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#a803897f6bae25dece4d7e23330f0f9da">g2o::OptimizableGraph::Vertex::clearQuadraticForm</a></div><div class="ttdeci">virtual void clearQuadraticForm()=0</div><div class="ttdoc">set the b vector part of this vertex to zero </div></div>
<div class="ttc" id="classg2o_1_1_jacobian_workspace_html"><div class="ttname"><a href="classg2o_1_1_jacobian_workspace.html">g2o::JacobianWorkspace</a></div><div class="ttdoc">provide memory workspace for computing the Jacobians </div><div class="ttdef"><b>Definition:</b> jacobian_workspace.h:50</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_html_af718590d044a1b44e4079136c29efcd1"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix.html#af718590d044a1b44e4079136c29efcd1">g2o::SparseBlockMatrix::rowBaseOfBlock</a></div><div class="ttdeci">int rowBaseOfBlock(int r) const</div><div class="ttdoc">where does the row at block-row r starts? </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix.h:108</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_vertex_html_aa605891d46b99ed97accf659f10bc8d5"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_vertex.html#aa605891d46b99ed97accf659f10bc8d5">g2o::OptimizableGraph::Vertex::setColInHessian</a></div><div class="ttdeci">void setColInHessian(int c)</div><div class="ttdoc">set the row of this vertex in the Hessian </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:321</div></div>
<div class="ttc" id="classg2o_1_1_hyper_graph_html_a5e2970e236c0dcb4eff7c205d7b6b4ae"><div class="ttname"><a href="classg2o_1_1_hyper_graph.html#a5e2970e236c0dcb4eff7c205d7b6b4ae">g2o::HyperGraph::EdgeSet</a></div><div class="ttdeci">std::set&lt; Edge * &gt; EdgeSet</div><div class="ttdef"><b>Definition:</b> hyper_graph.h:90</div></div>
<div class="ttc" id="structg2o_1_1_g2_o_batch_statistics_html_ae2af64720e3f99557924b9cf92ab6f20"><div class="ttname"><a href="structg2o_1_1_g2_o_batch_statistics.html#ae2af64720e3f99557924b9cf92ab6f20">g2o::G2OBatchStatistics::hessianPoseDimension</a></div><div class="ttdeci">size_t hessianPoseDimension</div><div class="ttdoc">dimension of the pose matrix in Schur </div><div class="ttdef"><b>Definition:</b> batch_stats.h:68</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a717fa8cb1dd5a212e41d8ebef67955e6"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a717fa8cb1dd5a212e41d8ebef67955e6">g2o::BlockSolver::LinearSolverType</a></div><div class="ttdeci">Traits::LinearSolverType LinearSolverType</div><div class="ttdef"><b>Definition:</b> block_solver.h:111</div></div>
<div class="ttc" id="structg2o_1_1_optimizable_graph_html_aa669dbd1d6e34e49fecda711ff1b78c6"><div class="ttname"><a href="structg2o_1_1_optimizable_graph.html#aa669dbd1d6e34e49fecda711ff1b78c6">g2o::OptimizableGraph::jacobianWorkspace</a></div><div class="ttdeci">JacobianWorkspace &amp; jacobianWorkspace()</div><div class="ttdoc">the workspace for storing the Jacobians of the graph </div><div class="ttdef"><b>Definition:</b> optimizable_graph.h:659</div></div>
<div class="ttc" id="classg2o_1_1_linear_solver_html_aa44b40826d50203c8ce2ff258c34e030"><div class="ttname"><a href="classg2o_1_1_linear_solver.html#aa44b40826d50203c8ce2ff258c34e030">g2o::LinearSolver::solve</a></div><div class="ttdeci">virtual bool solve(const SparseBlockMatrix&lt; MatrixType &gt; &amp;A, double *x, double *b)=0</div><div class="ttdoc">Assumes that A is the same matrix for several calls. </div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_aab81798b80dcb6c4182fa1d510914234"><div class="ttname"><a href="classg2o_1_1_block_solver.html#aab81798b80dcb6c4182fa1d510914234">g2o::BlockSolver::writeDebug</a></div><div class="ttdeci">virtual bool writeDebug() const</div><div class="ttdef"><b>Definition:</b> block_solver.h:138</div></div>
<div class="ttc" id="classg2o_1_1_solver_html_a52c92c9bf5db0da3322da3a02dbeb245"><div class="ttname"><a href="classg2o_1_1_solver.html#a52c92c9bf5db0da3322da3a02dbeb245">g2o::Solver::_b</a></div><div class="ttdeci">double * _b</div><div class="ttdef"><b>Definition:</b> solver.h:137</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_c_c_s_html_aa687d275090226a00f2685d66d134fcd"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_c_c_s.html#aa687d275090226a00f2685d66d134fcd">g2o::SparseBlockMatrixCCS::rowBaseOfBlock</a></div><div class="ttdeci">int rowBaseOfBlock(int r) const</div><div class="ttdoc">where does the row at block-row r start? </div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_ccs.h:88</div></div>
<div class="ttc" id="classg2o_1_1_sparse_optimizer_html_a93e11150d513979a6367518ecaf26e02"><div class="ttname"><a href="classg2o_1_1_sparse_optimizer.html#a93e11150d513979a6367518ecaf26e02">g2o::SparseOptimizer::indexMapping</a></div><div class="ttdeci">const VertexContainer &amp; indexMapping() const</div><div class="ttdoc">the index mapping of the vertices </div><div class="ttdef"><b>Definition:</b> sparse_optimizer.h:191</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a662473598270cdf89075607f87440759"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a662473598270cdf89075607f87440759">g2o::BlockSolver::updateStructure</a></div><div class="ttdeci">virtual bool updateStructure(const std::vector&lt; HyperGraph::Vertex *&gt; &amp;vset, const HyperGraph::EdgeSet &amp;edges)</div><div class="ttdoc">update the structures for online processing </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:298</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a8bf01018abc3bfddfa3b29a380a1d6cb"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a8bf01018abc3bfddfa3b29a380a1d6cb">g2o::BlockSolver::init</a></div><div class="ttdeci">virtual bool init(SparseOptimizer *optmizer, bool online=false)</div><div class="ttdoc">initialize the solver, called once before the first iteration </div><div class="ttdef"><b>Definition:</b> block_solver.hpp:607</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a96bf60b923f816086cd2f24de38736ec"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a96bf60b923f816086cd2f24de38736ec">g2o::BlockSolver::PoseLandmarkMatrixType</a></div><div class="ttdeci">Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType</div><div class="ttdef"><b>Definition:</b> block_solver.h:104</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a0075af2df18364cf99fd80f813b8ce4b"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a0075af2df18364cf99fd80f813b8ce4b">g2o::BlockSolver::resize</a></div><div class="ttdeci">void resize(int *blockPoseIndices, int numPoseBlocks, int *blockLandmarkIndices, int numLandmarkBlocks, int totalDim)</div><div class="ttdef"><b>Definition:</b> block_solver.hpp:65</div></div>
<div class="ttc" id="classg2o_1_1_optimizable_graph_1_1_edge_html_a0fdad5ebfb4efec9f893b57f67e0fbe1"><div class="ttname"><a href="classg2o_1_1_optimizable_graph_1_1_edge.html#a0fdad5ebfb4efec9f893b57f67e0fbe1">g2o::OptimizableGraph::Edge::linearizeOplus</a></div><div class="ttdeci">virtual void linearizeOplus(JacobianWorkspace &amp;jacobianWorkspace)=0</div><div class="ttdoc">Linearizes the constraint in the edge in the manifold space, and store the result in the given worksp...</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_c_c_s_html"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_c_c_s.html">g2o::SparseBlockMatrixCCS&lt; PoseLandmarkMatrixType &gt;</a></div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_ac222d4342825ed8632a87b4f5be94618"><div class="ttname"><a href="classg2o_1_1_block_solver.html#ac222d4342825ed8632a87b4f5be94618">g2o::BlockSolver::_Hpp</a></div><div class="ttdeci">SparseBlockMatrix&lt; PoseMatrixType &gt; * _Hpp</div><div class="ttdef"><b>Definition:</b> block_solver.h:150</div></div>
<div class="ttc" id="classg2o_1_1_hyper_graph_1_1_edge_html_ab644c1c4e38a0112db1435fbc0233f25"><div class="ttname"><a href="classg2o_1_1_hyper_graph_1_1_edge.html#ab644c1c4e38a0112db1435fbc0233f25">g2o::HyperGraph::Edge::vertex</a></div><div class="ttdeci">const Vertex * vertex(size_t i) const</div><div class="ttdoc">returns the pointer to the ith vertex connected to the hyper-edge. </div><div class="ttdef"><b>Definition:</b> hyper_graph.h:140</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_a3bc5b19faa2c45e2c04a6743b3a083de"><div class="ttname"><a href="classg2o_1_1_block_solver.html#a3bc5b19faa2c45e2c04a6743b3a083de">g2o::BlockSolver::_diagonalBackupLandmark</a></div><div class="ttdeci">std::vector&lt; LandmarkVectorType, Eigen::aligned_allocator&lt; LandmarkVectorType &gt; &gt; _diagonalBackupLandmark</div><div class="ttdef"><b>Definition:</b> block_solver.h:163</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_afd898a666343291129d37a979e23ded6"><div class="ttname"><a href="classg2o_1_1_block_solver.html#afd898a666343291129d37a979e23ded6">g2o::BlockSolver::LandmarkMatrixType</a></div><div class="ttdeci">Traits::LandmarkMatrixType LandmarkMatrixType</div><div class="ttdef"><b>Definition:</b> block_solver.h:103</div></div>
<div class="ttc" id="classg2o_1_1_sparse_block_matrix_diagonal_html_ae7d969c12506cdcd1b7d54128f3b6985"><div class="ttname"><a href="classg2o_1_1_sparse_block_matrix_diagonal.html#ae7d969c12506cdcd1b7d54128f3b6985">g2o::SparseBlockMatrixDiagonal::multiply</a></div><div class="ttdeci">void multiply(double *&amp;dest, const double *src) const</div><div class="ttdef"><b>Definition:</b> sparse_block_matrix_diagonal.h:77</div></div>
<div class="ttc" id="classg2o_1_1_block_solver_html_ad6a1a8f17c8fb854962a8204c79bc981"><div class="ttname"><a href="classg2o_1_1_block_solver.html#ad6a1a8f17c8fb854962a8204c79bc981">g2o::BlockSolver::_DInvSchur</a></div><div class="ttdeci">SparseBlockMatrixDiagonal&lt; LandmarkMatrixType &gt; * _DInvSchur</div><div class="ttdef"><b>Definition:</b> block_solver.h:155</div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="dir_08235c883c2d11e586a63e6645937db5.html">Thirdparty</a></li><li class="navelem"><a class="el" href="dir_6238d635334f7e809f450ee18181fd21.html">g2o</a></li><li class="navelem"><a class="el" href="dir_62a53d30c17c36057e41412a1a4f77b8.html">g2o</a></li><li class="navelem"><a class="el" href="dir_b7aaf3f9367a0f07449e9b09401b4637.html">core</a></li><li class="navelem"><a class="el" href="block__solver_8hpp.html">block_solver.hpp</a></li>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.12 </li>
  </ul>
</div>
</body>
</html>
